{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "import time\n",
    "import random\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "plt.style.use({'figure.figsize':(10, 10)})\n",
    "pd.set_option('max_rows', 300)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_Rate=0.000001\n",
    "Velocity_tripod=0.289*40\n",
    "Velocity_quadruped=0.115*40\n",
    "Velocity_onebyone=0.096*40\n",
    "Up_degree=np.array([-40,-20,0,20,40])\n",
    "Left_degree=np.array([-60,-80,-100,-120])\n",
    "Right_degree=np.array([60,80,100,120]) #\n",
    "Robot_r=40 #the radius of hexapod robot\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the map, 'RGB' mode is used to reprensent different colors.\n",
    "\n",
    "(0,0,0)---Black---Obstacle and margin\n",
    "\n",
    "(255,255,255)---White---Flat ground\n",
    "\n",
    "(255,0,0)---Red---Destination\n",
    "\n",
    "(0,255,255)---Cyan---Hexapod robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map4(Destination_x,Destination_y):\n",
    "    im4=Image.new('RGB',size=(3000,2000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im4,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,1960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,800,1000,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,800,800,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2000,800,800,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,250,800,500),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((2500,200,2700,1000),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((1100,300,2000,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2200,1550,2700,1850),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im4\n",
    "im4=Draw_map4(1750,500)\n",
    "plt.imshow(im4)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "States=np.array(['L60D0/30/H0','L180D0/30/H0','L250D0/30/H0','L300D0/30/H0',\n",
    "'L60D30/60/H0','L180D30/60/H0','L250D30/60/H0','L300D30/60/H0',\n",
    "'L60D60/90/H0','L180D60/90/H0','L250D60/90/H0','L300D60/90/H0',\n",
    "'L60D90/120/H0','L180D90/120/H0','L250D90/120/H0','L300D90/120/H0',\n",
    "'L60D120/150/H0','L180D120/150/H0','L250D120/150/H0','L300D120/150/H0',\n",
    "'L60D150/180/H0','L180D150/180/H0','L250D150/180/H0','L300D150/180/H0',\n",
    "'L60D180/210/H0','L180D180/210/H0','L250D180/210/H0','L300D180/210/H0',\n",
    "'L60D210/240/H0','L180D210/240/H0','L250D210/240/H0','L300D210/240/H0',\n",
    "'L60D240/270/H0','L180D240/270/H0','L250D240/270/H0','L300D240/270/H0',\n",
    "'L60D270/300/H0','L180D270/300/H0','L250D270/300/H0','L300D270/300/H0',\n",
    "'L60D300/330/H0','L180D300/330/H0','L250D300/330/H0','L300D300/330/H0',\n",
    "'L60D330/360/H0','L180D330/360/H0','L250D330/360/H0','L300D330/360/H0',\n",
    "'L60D0/30/H1','L180D0/30/H1','L250D0/30/H1','L300D0/30/H1',\n",
    "'L60D30/60/H1','L180D30/60/H1','L250D30/60/H1','L300D30/60/H1',\n",
    "'L60D60/90/H1','L180D60/90/H1','L250D60/90/H1','L300D60/90/H1',\n",
    "'L60D90/120/H1','L180D90/120/H1','L250D90/120/H1','L300D90/120/H1',\n",
    "'L60D120/150/H1','L180D120/150/H1','L250D120/150/H1','L300D120/150/H1',\n",
    "'L60D150/180/H1','L180D150/180/H1','L250D150/180/H1','L300D150/180/H1',\n",
    "'L60D180/210/H1','L180D180/210/H1','L250D180/210/H1','L300D180/210/H1',\n",
    "'L60D210/240/H1','L180D210/240/H1','L250D210/240/H1','L300D210/240/H1',\n",
    "'L60D240/270/H1','L180D240/270/H1','L250D240/270/H1','L300D240/270/H1',\n",
    "'L60D270/300/H1','L180D270/300/H1','L250D270/300/H1','L300D270/300/H1',\n",
    "'L60D300/330/H1','L180D300/330/H1','L250D300/330/H1','L300D300/330/H1',\n",
    "'L60D330/360/H1','L180D330/360/H1','L250D330/360/H1','L300D330/360/H1',\n",
    "'L60D0/30/H2','L180D0/30/H2','L250D0/30/H2','L300D0/30/H2',\n",
    "'L60D30/60/H2','L180D30/60/H2','L250D30/60/H2','L300D30/60/H2',\n",
    "'L60D60/90/H2','L180D60/90/H2','L250D60/90/H2','L300D60/90/H2',\n",
    "'L60D90/120/H2','L180D90/120/H2','L250D90/120/H2','L300D90/120/H2',\n",
    "'L60D120/150/H2','L180D120/150/H2','L250D120/150/H2','L300D120/150/H2',\n",
    "'L60D150/180/H2','L180D150/180/H2','L250D150/180/H2','L300D150/180/H2',\n",
    "'L60D180/210/H2','L180D180/210/H2','L250D180/210/H2','L300D180/210/H2',\n",
    "'L60D210/240/H2','L180D210/240/H2','L250D210/240/H2','L300D210/240/H2',\n",
    "'L60D240/270/H2','L180D240/270/H2','L250D240/270/H2','L300D240/270/H2',\n",
    "'L60D270/300/H2','L180D270/300/H2','L250D270/300/H2','L300D270/300/H2',\n",
    "'L60D300/330/H2','L180D300/330/H2','L250D300/330/H2','L300D300/330/H2',\n",
    "'L60D330/360/H2','L180D330/360/H2','L250D330/360/H2','L300D330/360/H2'])\n",
    "Actions=np.array(['FrontH0','FrontH1','FrontH2','Left_45D','Right_45D'])\n",
    "Length_state=len(States)\n",
    "Length_action=len(Actions)\n",
    "Q_table=np.zeros((Length_state,Length_action))\n",
    "Q_table=pd.DataFrame(Q_table,columns=Actions,index=States)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Initial_Q_Table(Length_action,Length_state):\n",
    "    Q_Table=np.zeros((Length_state,Length_action))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-Table!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_Table"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Set_destination(x_range,y_range):\n",
    "    x=np.random.randint(300,x_range-300)\n",
    "    y=np.random.randint(300,y_range-300)\n",
    "    return int(x),int(y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Robot_destination(Destination_x,Destination_y,Robot_x,Robot_y,Robot_a):\n",
    "    Delta_x=Destination_x-Robot_x\n",
    "    Delta_y=Destination_y-Robot_y\n",
    "    Distance=np.sqrt(Delta_x*Delta_x+Delta_y*Delta_y)\n",
    "    Direct_angle=(np.arctan(Delta_y/Delta_x))*180/np.pi\n",
    "    if Delta_x<0:\n",
    "        Direct_angle=Direct_angle+180\n",
    "    True_angle=(Robot_a-Direct_angle)%360\n",
    "    if True_angle>180:\n",
    "        return 360-True_angle,'right',Distance\n",
    "    else:\n",
    "        return True_angle,'left',Distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,50,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash\n",
    "# Is_Crash(10,10,im4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Is_arrive(Current_x,Current_y,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "#     Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Degree=[-60,-30,0,30,60]\n",
    "    Dis=np.arange(0,30,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "            if (im.getpixel((x,y)))==(255,0,0):\n",
    "                Arrive=True\n",
    "                break\n",
    "        if Arrive==True or Crash==True:\n",
    "                break\n",
    "    return Arrive\n",
    "# print(Is_arrive(1750,500,im4))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Random_start(x_range,y_range,im):\n",
    "    a=0\n",
    "    x=np.random.random()*x_range\n",
    "    y=np.random.random()*y_range\n",
    "    while(Is_Crash(x,y,im)==True or Is_arrive(x,y,im)==True):\n",
    "        x=np.random.random()*x_range\n",
    "        y=np.random.random()*y_range\n",
    "    return x,y,a"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Height_level(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[-60,-30,0,30,60]\n",
    "    Distance=np.arange(0,50,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level\n",
    "# Height_level(750,1000,im4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_state_index(True_angle,Left_right,Distance,Height):\n",
    "    if 0<=Distance<60:\n",
    "        Dis_level=0\n",
    "    elif 60<=Distance<180:\n",
    "        Dis_level=1\n",
    "    elif 180<=Distance<250:\n",
    "        Dis_level=2\n",
    "    else:\n",
    "        Dis_level=3\n",
    "    Angle_level=True_angle//30\n",
    "    if Left_right=='right':\n",
    "        Le_ri=1\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+(5-Angle_level)*4)\n",
    "    elif Left_right=='left':\n",
    "        Le_ri=0\n",
    "        Angle_level=True_angle//30\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+Angle_level*4)\n",
    "    return State_number,Dis_level,Angle_level\n",
    "A,B,C=Output_state_index(80,'right',110,1)\n",
    "States[A]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Choose_action(Q_Table,Current_state,Action_times):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times)\n",
    "    State_action=Q_Table[Current_state,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action==[0])):\n",
    "        Next_action=np.random.randint(Length_action)\n",
    "    else:\n",
    "        Next_action=np.argmax(State_action)\n",
    "    return Next_action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Next_state_F(Robot_x,Robot_y,Robot_a,Destination_x,Destination_y,Action,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Current_height=Height_level(Robot_x,Robot_y,Robot_a,im)\n",
    "    Action_level=0\n",
    "    \n",
    "    if Action==0:\n",
    "        Robot_next_x=Robot_x+Velocity_tripod*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_tripod*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==1:\n",
    "        Robot_next_x=Robot_x+Velocity_quadruped*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_quadruped*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==2:\n",
    "        Robot_next_x=Robot_x+Velocity_onebyone*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_onebyone*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==3:\n",
    "        Robot_a=Robot_a-45\n",
    "        Robot_next_x=Robot_x\n",
    "        Robot_next_y=Robot_y\n",
    "    elif Action==4:\n",
    "        Robot_a=Robot_a+45\n",
    "        Robot_next_x=Robot_x\n",
    "        Robot_next_y=Robot_y\n",
    "        \n",
    "   \n",
    "    \n",
    "    Next_true_angle,Next_le_ri,Next_distance=Robot_destination(Destination_x,Destination_y,Robot_next_x,Robot_next_y,Robot_a)\n",
    "    Next_height=Height_level(Robot_next_x,Robot_next_y,Robot_a,im)\n",
    "    Next_state_number,Next_distance_level,Next_angle_level=Output_state_index(Next_true_angle,Next_le_ri,Next_distance,Next_height)\n",
    "    \n",
    "    if (Current_height==2 and Action==0) or (Current_height==2 and Action==1):\n",
    "        Reward=-500\n",
    "    elif(Current_height==1 and Action==0) or (Current_height==1 and Action==2):\n",
    "        Reward=-500\n",
    "    elif(Current_height==0 and Action==1) or (Current_height==0 and Action==2):\n",
    "        Reward=-500\n",
    "    else:\n",
    "        if(Actions==1 or Actions==2 or Actions==3):\n",
    "            Action_level=5\n",
    "        if Arrive==False:\n",
    "            Reward=Action_level+(3-Next_distance_level)+(6-Next_angle_level)*0.5\n",
    "        else:\n",
    "            Reward=80\n",
    "    return Robot_next_x,Robot_next_y,Robot_a,Reward,Next_state_number,Arrive,Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_Table=Initial_Q_Table(Length_action,Length_state)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times\n",
    "Action_times=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Run_function():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Vector_x\n",
    "    global Vector_y\n",
    "    Crash_=False\n",
    "    Arrive_=False\n",
    "    Destination_x,Destination_y=Set_destination(3000,2000)\n",
    "    im4=Draw_map4(Destination_x,Destination_y)\n",
    "    plt.imshow(im4)\n",
    "    plt.show()\n",
    "    Current_x,Current_y,Current_a=Random_start(1000,2000,im4)\n",
    "    Epoche_false=0\n",
    "#     Vector_x=[]\n",
    "#     Vector_y=[]\n",
    "#     Vector_x.append(Current_x)\n",
    "#     Vector_y.append(Current_y)\n",
    "    while Epoche_false<2000:\n",
    "\n",
    "        True_a,Le_ri,Distance=Robot_destination(Destination_x,Destination_y,Current_x,Current_y,Current_a)\n",
    "        Height_level_=Height_level(Current_x,Current_y,Current_a,im4)\n",
    "        Current_state_number,Distance_level,Angle_level=Output_state_index(True_a,Le_ri,Distance,Height_level_)\n",
    "        Next_action=Choose_action(Q_Table,Current_state_number,Action_times)\n",
    "        Next_x,Next_y,Next_a,Reward,Next_state_number,Arrive,Crash=Next_state_F(Current_x,Current_y,Current_a,Destination_x,Destination_y,Next_action,im4)\n",
    "        if Crash==True:\n",
    "            Next_x,Next_y,Next_a=Random_start(3000,2000,im4)\n",
    "        else:\n",
    "            if Arrive==True:\n",
    "                Next_true_a,Next_lr,Next_distance=Robot_destination(Destination_x,Destination_y,Next_x,Next_y,Next_a)\n",
    "                print('**********************************')\n",
    "                print('Destination_x=%f Destination_y=%f'%(Destination_x,Destination_y))\n",
    "                print('last_x=%f, last_y=%f,last_a=%f'%(Current_x,Current_y,Current_a%360))\n",
    "                print('Next_state_number=%d Reward=%f'%(Next_state_number,Reward))\n",
    "                print('Next_x=%f Next_y=%f True_angle=%f DestinationRobotDistance=%f Action=%d State=%s'%(Next_x,Next_y,Next_true_a,Next_distance,Next_action,States[Next_state_number]))\n",
    "                print('DestinationRobotAngle=%f Robot_current_angle=%d'%((np.arctan((Next_y-Destination_y)/(Next_x-Destination_x)))*180/np.pi,Next_a%360))\n",
    "                print('Epsilon=%f Action_times=%d Epoche_false=%d'%(Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times),Action_times,Epoche_false))\n",
    "                Destination_x,Destination_y=Set_destination(3000,2000)\n",
    "                im4=Draw_map4(Destination_x,Destination_y)\n",
    "#                 plt.imshow(im4)\n",
    "#                 plt.show()\n",
    "                Next_x,Next_y,Next_a=Random_start(3000,2000,im4)\n",
    "                Epoche_false+=1\n",
    "                Epoche+=1\n",
    "            Q_target=Reward+Beta*max(Q_Table[Next_state_number])\n",
    "            Q_Table[Current_state_number][Next_action]+=Alpha*(Q_target-Q_Table[Current_state_number][Next_action])\n",
    "#             Vector_x.append(Current_x)\n",
    "#             Vector_y.append(Current_y)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_a=Next_a\n",
    "        Action_times+=1\n",
    "    return Q_Table\n",
    "Q_Table_final=Run_function()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "Q_Table_=pd.DataFrame(Q_Table_final,columns=Actions,index=States)\n",
    "Q_Table_"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
